package team3132.commands;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.command.Command;
import team3132.subsystems.DriveSubsystem;
import team3132.utils.TimeDelayCheck;
import team3132.utils.Value;

/**
 * Implements joystick driving of the drive base.
 */
public class DriveByJoysticksCommand extends Command {

  private final Joystick leftStick;
  private final Joystick rightStick;
  private final TimeDelayCheck timeDelayCheck;
  private final DriveSubsystem driveSubsystem;

  public DriveByJoysticksCommand(
          DriveSubsystem driveSubsystem, Joystick leftStick, Joystick rightStick, Value timeout) {
    this.driveSubsystem = driveSubsystem;
    this.leftStick = leftStick;
    this.rightStick = rightStick;
    this.timeDelayCheck = new TimeDelayCheck("DriveByJoysticks", timeout.getDoubleValue());
    requires(driveSubsystem);
    this.setInterruptible(false);
  }

  // Called just before this Command runs the first time
  protected void initialize() {
    driveSubsystem.resetLeftEncoderDistance();
    driveSubsystem.resetRightEncoderDistance();
    timeDelayCheck.initTimeCheck();
  }

  // Called repeatedly when this Command is scheduled to run
  protected void execute() {
    double move = -removeDeadband(leftStick.getY());
    double rotate = -removeDeadband(rightStick.getX());
    driveSubsystem.arcadeDrive(move, rotate);
  }

  // Removes almost zero values so that if joysticks don't automatically
  // center the motors will actually stop.
  private double removeDeadband(double value) {
    if (Math.abs(value) < 0.15) {
      value = 0;
    }
    return value;
  }

  // Make this return true when this Command no longer needs to run execute()
  protected boolean isFinished() {
    return false;
  }

  // Called once after isFinished returns true
  protected void end() {
  }

  // Called when another command which requires one or more of the same
  // subsystems is scheduled to run
  protected void interrupted() {
  }
}
